{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Number of correspondences loaded =  95\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import cv2\n",
    "import pygcransac\n",
    "from time import time\n",
    "\n",
    "correspondences = np.loadtxt('img/pose6dscene_points.txt')\n",
    "gt_pose = np.loadtxt('img/pose6dscene_gt.txt')\n",
    "intrinsic_params = np.loadtxt('img/pose6dscene.K')\n",
    "\n",
    "print(\"Number of correspondences loaded = \", str(len(correspondences)))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def verify_pygcransac(corrs, K):    \n",
    "    n = len(correspondences)\n",
    "    imagePoints = np.float32([corrs[i][0:2] for i in np.arange(n)]).reshape(-1,2)\n",
    "    worldPoints = np.float32([corrs[i][2:5] for i in np.arange(n)]).reshape(-1,3)\n",
    "    \n",
    "    threshold = 5.5\n",
    "    normalized_threshold = threshold / (K[0, 0] + K[1, 1]) / 2.0;    \n",
    "    pose, mask = pygcransac.find6DPose(\n",
    "        np.ascontiguousarray(imagePoints), \n",
    "        np.ascontiguousarray(worldPoints), \n",
    "        normalized_threshold, 0.999)    \n",
    "    return pose, mask\n",
    "\n",
    "def normalize_image_points(corrs, K): \n",
    "    n = len(corrs)\n",
    "    normalized_correspondences = np.zeros((corrs.shape[0], 5))\n",
    "    inv_K = np.linalg.inv(K)\n",
    "\n",
    "    for i in range(n):\n",
    "        p1 = np.append(correspondences[i][0:2], 1)\n",
    "        p2 = inv_K.dot(p1)\n",
    "        normalized_correspondences[i][0:2] = p2[0:2]\n",
    "        normalized_correspondences[i][2:] = correspondences[i][2:]\n",
    "    return normalized_correspondences\n",
    "\n",
    "def calculate_error(gt_pose, est_pose):\n",
    "    \n",
    "    R2R1 = np.dot(gt_pose[:, 0:3].T, est_pose[:, 0:3])\n",
    "    cos_angle = max(-1.0, min(1.0, 0.5 * (R2R1.trace() - 1.0)))\n",
    "    \n",
    "    err_R = np.arccos(cos_angle) * 180.0 / np.pi\n",
    "    err_t = np.linalg.norm(gt_pose[:, 3] - est_pose[:, 3])\n",
    "    \n",
    "    return err_R, err_t\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0.01994633674621582  sec gc-ransac\n",
      "Rotation error =  1.8860476299618448e-05 °\n",
      "Translation error =  0.0001007179771463513  mm\n"
     ]
    }
   ],
   "source": [
    "normalized_correspondences = normalize_image_points(correspondences, intrinsic_params)\n",
    "\n",
    "t = time()\n",
    "\n",
    "mag_pose, mag_mask = verify_pygcransac(normalized_correspondences, intrinsic_params)\n",
    "print (time()-t, ' sec gc-ransac')\n",
    "\n",
    "err_R, err_t = calculate_error(gt_pose, mag_pose)\n",
    "\n",
    "print ('Rotation error = ', err_R, '°')\n",
    "print ('Translation error = ', err_t, ' mm')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
